#include "std_msgs/String.h"
#include <sstream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
// for Wifibot
#include "robotControl/WStatus.h" //define the message type of vn100
// for Vector Nav
#include "robotControl/vn100_msg.h" //define the message type of vn100
#include "robotControl/speed_msg.h" //define the message type Speed for passing it to the wifibot
#include "robotControl/VnQuaternion_msg.h" //define the message type of VectorSpace(Quaternion To get X ,Y, Z, W) 
#include "robotControl/VnVector3_msg.h" //define the message type of 3 Compnenets used to get acceleration magnetic values through VN
#include "robotControl/positionWifibot_msg.h" //define the message type Speed for passing it to the wifibot
#include "robotControl/PointCloudWithOrigin.h" // for positioning through URG laser scanner
#include "geometry_msgs/Point32.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
 #include "tf/transform_datatypes.h"
#include "robotControl/KalmanFilteredPosition_msg.h" // for kalman filter messages from vectornav
 
// for subscriber handling
#include <ros/callback_queue.h>
// for async threads
#include <boost/thread.hpp>
// for speed control
#define SPEED_FORWARD     2.0
#define SPEED_FORWARD_MS  0.2

#define SPEED_BACKWARD    220
#define SPEED_BACKWARD_MS  0.2

#define SPEED_MS  0.2

#define NO_OF_ITERATIONS 40
#define SET_TO_ZERO      0

#define SET_MOTOR_IN_THREAD_MODE 1
#define SET_MOTOR_INLINE_MODE 2

#define VN_YAW_OFFSET 9
#define METER_TO_CM 100
#define ODO_ONE_METER 4900
#define DEFAULT_SPEED 2
#define DEFAULT_ROTATE_SPEED 6
#define CLOSED_LOOP_SPEED_FORWARD 192  // 128+1*64
#define CLOSED_LOOP_SPEED_BACKWARD 128 // 128+ 0*64
#define OPEN_LOOP_SPEED_FORWARD 64     // 0+1*64
#define OPEN_LOOP_SPEED_BACKWARD 0     // 0+ 0*64
#define ODO_ONE_DEGREE 17
#define PI 3.14159265


#define ODO_ZERO_DEGREE 0
#define ODO_FORTYFIVE_DEGREE 900
#define ODO_SIXTY_DEGREE 1150
#define ODO_NINTY_DEGREE 2000
#define ODO_ONEEIGHTY_DEGREE 4500


//Define new type for distances
typedef  float DISTANCE;
//Define new type for odometry
typedef double ODOMETRY;
//Define new type for Bearing
typedef  double BEARING;
//Define new type for speed
typedef float SPEED;
//using namespace std;
//Define new type for Bearing
typedef  double COORDINATE;


class wifibotControl{
public:
    // Create an publisher object for publishing the speed
    static ros::Publisher _speedPublisher;
    //Create a message object for the publishing the speed 
    static robotControl::speed_msg topicSpeed; 
   //Create a message object for the publishing the Wifibot Status 
    static robotControl::WStatus wifibotGlobalStatus;
   //Create a message object for the publishing the VN Oreintation Status 
    static robotControl::vn100_msg vnGlobalStatus;
   //Create a message object for the publishing the Vector Space and Quaternion Status 
     static geometry_msgs::Quaternion quaternionStatus;
   //Create a message object for the publishing the VN Angular Status 
     static geometry_msgs::Vector3 angularRateStatus;
   //Create a message object for the publishing the VN Acceleration Status 
   static geometry_msgs::Vector3 accelerationStatus;
    //Create a message object for the publishing the Wifibot position 
    static nav_msgs::Odometry wifibotPose;
    //Create a message object for the wifibot position  
    static robotControl::positionWifibot_msg positionWbtStatus;
    //Create a message object for  PointCloudWithOrigin for URG positioning
    static robotControl::PointCloudWithOrigin positionFromURG;
   //Create a message object for  IMU Data
    static sensor_msgs::Imu messageImu;
     //Create a message object for  KalmanFilter Data
    static robotControl::KalmanFilteredPosition_msg messageKalmanFiltered;
       //Create a message object for  robot_pose_ekf Position data
    static geometry_msgs::PoseWithCovarianceStamped EkfPoseData;
    
    
    //Declare Subscriber for the Robot Status
    static ros::Subscriber subscriberStatus;
    //Declare Subscriber for the Odometry (Wifibot Position)
    static ros::Subscriber subscriberOdom;
    //Declare Subscriber for VectorNav
    static ros::Subscriber subscriberVN100;
    //Declare Subscriber for Quanternion (Vector Space)
    static ros::Subscriber subscriberQuan;
    //Declare Subscriber for Acceleration
    static ros::Subscriber subscriberAccel ;
    //Declare Subscriber for Angular Rate
    static ros::Subscriber subscriberAngRate;
    //Declare Subscriber for wifibot Position
    static ros::Subscriber subscriberWifibotPosition;
    //Declare Subscriber for IMU data
    static ros::Subscriber subscriberImuData;
      //Declare Subscriber for Kalman filter Position
    static ros::Subscriber subscriberKalmanFilterData;
      //Declare Subscriber for EKF robot_pose_ekf library
    static ros::Subscriber subscriberEkfPoseData;
    
    //Callback function for the robot status subscriber
      void statusCallback(robotControl::WStatus msg);
    //Callback function for the robot position publisher
      void positionStatusCallback(robotControl::positionWifibot_msg msg);
     //Callback function for the vectornav subscriber
      void vnStatusCallback(robotControl::vn100_msg msg);
    //Callback function for the odometry subscriber
     void odomCallback(const nav_msgs::Odometry odom);
     //Callback function for the VN Quaternion Subscriber
     void vnQuaternionCallback(geometry_msgs::Quaternion vSpaceMsg);
     //Callback function for the VN acceleration Subscriber
     void vnAccelerationCallback(geometry_msgs::Vector3  msg);
     //Callback function for the VN Angular Rate Subscriber
     void vnAngularRateCallback(geometry_msgs::Vector3  angulRateMsg);
      //Callback function for the IMU data Subscriber
     void vnIMUCallback(sensor_msgs::Imu msgImu);
       //Callback function for the Kalman Filter data Subscriber
     void vnKalmanCallback(robotControl::KalmanFilteredPosition_msg msgKFPos);
        //Callback function for the Kalman Filter data Subscriber
     void EkfPoseCallback(geometry_msgs::PoseWithCovarianceStamped msgEkfPose);
     
     
    //Print the control menu
    static void menu();
    //Print Wifibot Status and Position
    static int printWifibotStatus();
    // Move the robot for a certain distance with a certain speed
    //For backward, put negative value
    //Allowed values -1.2 until 1.2
    static int Move(DISTANCE distance, SPEED speed);

    //Allowed values -1.2 until 1.2
    static int Rotate(BEARING rotationAngle, SPEED speed);

};